/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <thread>

#include "dal_msg/dalidl_runtime_c/message_type_support_struct.h"
#include "dalidl_typesupport_introspection_cpp/identifier.hpp"
#include "serdata.hpp"
#include "TypeSupport2.hpp"

#include "dds/dds.h"

#include "mj_interfaces/msg/mjtype.hpp"
#include "cyclonedds_pubsub.hpp"

mj_interfaces::msg::Mjtype mjtype;

void cb(dds_entity_t reader, void *arg)
{
    dds_return_t rc;
    dds_sample_info_t info;
    void * ros_message = &mjtype;
    rc = dds_take(reader, &ros_message, &info, 1, 1);
    if (rc > 0) {
        if (!info.valid_data)
            return;

        std::cout << "on_data_available: "
            << "i8: " << (int)mjtype.i8 << ", "
            << "i16: " << mjtype.i16 << ", "
            << "str: " << mjtype.str << ", "
            << std::endl;

        fflush (stdout);
    }
}

int main(int argc, char **argv)
{
    dds_entity_t participant = dds_create_participant (20, NULL, NULL);
    if (participant < 0) {
        std::cout << "create participant failed";
        return 1;
    }

    const dalidl_message_type_support_t & type_supports = 
        get_message_type_support_handle<mj_interfaces::msg::Mjtype>();
    const dalidl_message_type_support_t * type_support = 
        get_message_typesupport_handle(&type_supports, dalidl_typesupport_introspection_cpp::typesupport_identifier);

    if (!type_support)
    {
        std::cout << "type_support is null";
        return 1;
    }

    bool is_fixed_type = is_type_self_contained(type_support);
    uint32_t sample_size = static_cast<uint32_t>(dal_cyclonedds_cpp::get_message_size(type_support));
    auto sertype = create_sertype(
        type_support->typesupport_identifier,
        create_message_type_support(type_support->data, type_support->typesupport_identifier), false,
        dal_cyclonedds_cpp::make_message_value_type(&type_supports), sample_size, is_fixed_type);
    
    ddsi_sertype * p = (ddsi_sertype *)sertype;
    dds_entity_t topic = dds_create_topic_sertype(participant, "Mj_Topic", 
        &p, nullptr, nullptr, nullptr);
    if (topic < 0) {
        std::cout << "create topic failed";
        ddsi_sertype_unref(sertype);
        return 1;
    }

    dds_entity_t publisher = dds_create_publisher(participant, NULL, NULL);
    if (publisher < 0) {
        std::cout << "create publisher failed";
        return 1;
    }

    dds_entity_t subscriber = dds_create_subscriber(participant, NULL, NULL);
    if (subscriber < 0) {
        std::cout << "create subscriber failed";
        return 1;
    }

    dds_entity_t datawriter = dds_create_writer(publisher, topic, NULL, NULL);
    if (datawriter < 0) {
        std::cout << "create datawriter failed";
        return 1;
    }

    dds_listener_t *c_listener = dds_create_listener(NULL);
    dds_lset_data_available(c_listener, cb);
    dds_entity_t datareader = dds_create_reader(subscriber, topic, NULL, c_listener);
    if (datareader < 0) {
        std::cout << "create datareader failed";
        return 1;
    }

    mj_interfaces::msg::Mjtype mj_type;
    mj_type.str = "mjtype";
    while (1) {
        mj_type.i8 += 1;
        mj_type.i16 += 1;
        dds_write(datawriter, &mj_type);
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    return 0;
}